#include "CalibrationDlg.h"
#include "QGView.h"

#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QPushButton>
#include <QTableWidget>
#include <QTableWidgetItem>
#include <QHeaderView>
#include <QFileDialog>
#include <QLabel>
#include <QLineEdit>

#include "VisionMainWnd.h"
#include "ModbusPoll.h"

#include "ICamera.h"

#include "Calibration_NinePoint.h"
#include "Visionapi.h"
#include "HalconCpp.h"
using namespace HalconCpp;

CalibrationDlg::CalibrationDlg(QWidget* parent)
{
	setParent(parent, windowFlags());

	setWindowFlags(windowFlags() | Qt::WindowMinimizeButtonHint | Qt::WindowMaximizeButtonHint);

	initUI();
}

void CalibrationDlg::initUI()
{
	QWidget* mainWidget = new QWidget(this);

	// 左边显示栏
	QWidget* leftArea = new QWidget(mainWidget);
	imgView_ = new QGView(mainWidget);
	imgView_->setObjectName("calibView");

	QVBoxLayout* leftLyt = new QVBoxLayout(leftArea);
	leftLyt->addWidget(imgView_);

	// 右边工具栏
	QWidget* rightArea = new QWidget(mainWidget);
	rightArea->setFixedWidth(350);
	QPushButton* btnGrabOne = new QPushButton("采集标定板", rightArea);
	QPushButton* btnLoadImage = new QPushButton("加载标定板图像", rightArea);
	QPushButton* btnProcess = new QPushButton("圆心提取", rightArea);
	QPushButton* btnAutoCalib = new QPushButton("自动标定", rightArea);

	// 9点坐标显示
	QTableWidget* calibData = new QTableWidget(rightArea);
	calibData->setObjectName("calibDataTab");
	auto tab = calibData;
	//tab->setEditTriggers(QAbstractItemView::NoEditTriggers);
	tab->setColumnCount(4);
	tab->setSelectionBehavior(QAbstractItemView::SelectRows);
	tab->setSelectionMode(QAbstractItemView::SingleSelection);

	QFont headerFont = tab->horizontalHeader()->font();
	headerFont.setBold(true);
	tab->horizontalHeader()->setFont(headerFont);

	//设置显示内容
	QStringList headerTitle;
	headerTitle << "px" << "py" << "rx" << "ry";
	tab->setHorizontalHeaderLabels(headerTitle);
	int colsWidth[] = { 60,60,60,60};
	for (int i = 0; i < 4; i++)
	{
		tab->setColumnWidth(i, colsWidth[i]);
	}

	QLabel* resultLab = new QLabel("标定误差RMS:", rightArea);
	QLineEdit* resultRms = new QLineEdit("", rightArea);
	resultRms->setObjectName("resultrms");
	QHBoxLayout* hLyt = new QHBoxLayout();
	hLyt->addWidget(resultLab);
	hLyt->addWidget(resultRms);

	// 标定按钮
	QPushButton* btnReadRobotData = new QPushButton("读取当前机器手坐标");
	QPushButton* btnTestData = new QPushButton("测试数据");
	QPushButton* btnCalib = new QPushButton("标定");
	QPushButton* btnSaveCalibData = new QPushButton("保存");

	QVBoxLayout* rightLyt = new QVBoxLayout(rightArea);
	rightLyt->addWidget(btnGrabOne);
	rightLyt->addWidget(btnLoadImage);
	rightLyt->addWidget(btnAutoCalib);
	rightLyt->addWidget(btnProcess);
	
	rightLyt->addWidget(btnReadRobotData);
	rightLyt->addWidget(btnTestData);
	rightLyt->addWidget(btnCalib);
	rightLyt->addWidget(btnSaveCalibData);

	rightLyt->addWidget(tab);
	rightLyt->addLayout(hLyt);
	rightLyt->addStretch();

	QHBoxLayout* mainLayout = new QHBoxLayout(mainWidget);
	mainLayout->addWidget(leftArea);
	mainLayout->addWidget(rightArea);
	setLayout(mainLayout);

	connect(btnGrabOne, &QPushButton::clicked, this, &CalibrationDlg::on_grab_one);
	connect(btnProcess, &QPushButton::clicked, this, &CalibrationDlg::on_process);
	connect(btnLoadImage, &QPushButton::clicked, this, &CalibrationDlg::on_load_image);
	connect(btnCalib, &QPushButton::clicked, this, &CalibrationDlg::on_calibration);
	connect(btnSaveCalibData, &QPushButton::clicked, this, &CalibrationDlg::on_save_calibData);
	connect(btnTestData, &QPushButton::clicked, this, &CalibrationDlg::on_test_data);
	connect(btnReadRobotData, &QPushButton::clicked, this, &CalibrationDlg::on_read_robData);

	btnTestData->setVisible(false);		// 测试功能取消
	btnAutoCalib->setVisible(false);	// 自动标定功能待实现
}

void CalibrationDlg::processCalibBoard(cv::Mat& srcImg)
{
	HImage img = VisionApi::HImageFromMat(srcImg);
	HRegion r = img.Threshold(0, 128);
	HRegion conn = r.Connection();
	HRegion openRegion = conn.OpeningCircle(3.5);
	HRegion regionCircle = openRegion.SelectShape("area", "and", 15000, 18000);
	HRegion sortRegion = regionCircle.SortRegion("character", "true", "row");

	// 转为彩色绘制
	if (srcImg.channels() == 1)
	{
		cv::Mat dstImg;
		cv::cvtColor(srcImg, dstImg, cv::COLOR_GRAY2BGR);
		srcImg = dstImg;
	}	
	
	auto tab = findChild<QTableWidget*>("calibDataTab");
	HTuple area, rows, cols;
	HalconCpp::AreaCenter(sortRegion, &area, &rows, &cols);
	double tempRow = rows[3];
	double tempCol = cols[3];
	rows[3] = rows[5];
	cols[3] = cols[5];
	rows[5] = tempRow;
	cols[5] = tempCol;
	for (int i = 0; i < area.Length(); i++)
	{
		VisionApi::drawCross(srcImg, cv::Point2d(cols[i].D(), rows[i].D()));
		cv::putText(srcImg,
			std::to_string(i).c_str(),
			cv::Point(cols[i].D(), rows[i].D()), // 确保位置不超出图像范围
			cv::FONT_HERSHEY_SIMPLEX,
			0.7,
			cv::Scalar(0, 0, 255), // 红色
			2);

		int row = tab->rowCount();
		tab->insertRow(row);
		QTableWidgetItem* item = new QTableWidgetItem;
		item->setText(QString::number(rows[i].D(), 'f', 2));
		tab->setItem(i, 0, item);

		item = new QTableWidgetItem;
		item->setText(QString::number(cols[i].D(), 'f', 2));
		tab->setItem(i, 1, item);		
	}

	tab->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);

	QImage dispImg = VisionApi::mat2QImage(srcImg);
	imgView_->DispImage(dispImg);
	imgView_->viewport()->update();
}

void CalibrationDlg::on_test_data()
{
//Row: = [100, 200, 300, 150, 250, 350, 180, 280, 380]
//Column : = [150, 150, 150, 250, 250, 250, 350, 350, 350]
//X_robot : = [50.0, 50.0, 50.0, 100.0, 100.0, 100.0, 150.0, 150.0, 150.0]
//Y_robot : = [50.0, 100.0, 150.0, 50.0, 100.0, 150.0, 50.0, 100.0, 150.0]
	double rows[] = { 100, 200, 300, 150, 250, 350, 180, 280, 380 };
	double cols[] = { 150, 150, 150, 250, 250, 250, 350, 350, 350 };
	double x_robot[] = { 50.0, 50.0, 50.0, 100.0, 100.0, 100.0, 150.0, 150.0, 150.0 };
	double y_robot[] = { 50.0, 100.0, 150.0, 50.0, 100.0, 150.0, 50.0, 100.0, 150.0 };

	auto tab = findChild<QTableWidget*>("calibDataTab");
	for (int i = 0; i < sizeof(rows)/sizeof(double) ; i++)
	{
		int row = tab->rowCount();
		tab->insertRow(row);

		QTableWidgetItem* item = new QTableWidgetItem;
		item->setText(QString::number(rows[i], 'f', 2));
		tab->setItem(i, 0, item);

		item = new QTableWidgetItem;
		item->setText(QString::number(cols[i], 'f', 2));
		tab->setItem(i, 1, item);

		item = new QTableWidgetItem;
		item->setText(QString::number(x_robot[i], 'f', 2));
		tab->setItem(i, 2, item);

		item = new QTableWidgetItem;
		item->setText(QString::number(y_robot[i], 'f', 2));
		tab->setItem(i, 3, item);
	}

	tab->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
}

void CalibrationDlg::on_read_robData()
{
	auto p = (VisionMainWnd*)parentWidget();
	auto mbPoll = p->getModbusPoll();
	uint16_t robotXY[] = {0,0 };
	int robotPosAddr = 31;
	int r = mbPoll->readRegisters(robotXY, robotPosAddr,2);
	if (r == 2)
	{
		qInfo() << "数据读取成功: " << robotXY[0] << "," << robotXY[1];

		auto tab = findChild<QTableWidget*>("calibDataTab");
		int row = tab->rowCount();
		if (row > 5)
		{
			QTableWidgetItem* item = new QTableWidgetItem;
			item->setText(QString::number(robotXY[0] / 10.0, 'f', 2));
			tab->setItem(readCount, 2, item);

			item = new QTableWidgetItem;
			item->setText(QString::number(robotXY[1] / 10.0, 'f', 2));
			tab->setItem(readCount, 3, item);
		}

		readCount++;
	}
	else
	{
		qInfo() << "数据读取失败，请检查通讯是否正常";
	}

}

void CalibrationDlg::on_load_image()
{
	QString imgPath = QFileDialog::getOpenFileName(this, "打开图像", "calib_data", "Image Format(*.png *.jpg *.bmp All(*.*))");
	if (imgPath.isEmpty())
	{
		qInfo() << "图像路径为空";
		return;
	}
	readCount = 0;

	srcImage_ = cv::imread(imgPath.toStdString().c_str(), cv::IMREAD_UNCHANGED);
	QImage img = VisionApi::mat2QImage(srcImage_);
	imgView_->DispImage(img);
	imgView_->viewport()->update();
}

void CalibrationDlg::on_grab_one()
{
	auto p = (VisionMainWnd*)parentWidget();

	p->pauseGrab(true);

	auto cam = p->getCamera();

	//cam->Grab();

	cam->ExecuteSoftwareTrigger();
	int ret = cam->getImageBuffer(srcImage_, 100);
	if (ret == 0)
	{
		qInfo() << "手动采集标定图像成功";
		QImage img = VisionApi::mat2QImage(srcImage_);
		imgView_->DispImage(img);
		imgView_->viewport()->update();
	}

	p->pauseGrab(false);
}

void CalibrationDlg::on_process()
{
	if (srcImage_.empty())
	{
		return;
	}

	clearTable();
	processCalibBoard(srcImage_);
}

void CalibrationDlg::on_calibration()
{
	auto tab = findChild<QTableWidget*>("calibDataTab");
	calib_nine_->clearDatas();	// 清除旧的标定数据
	// 获取像素点坐标：
	for (int i = 0; i < tab->rowCount(); i++)
	{
		double pos[4] = { 0 };
		for (int j = 0; j < 4; j++)
		{
			auto item = tab->item(i, j);
			pos[j] = item->text().toDouble();
		}

		calib_nine_->addPoint_pixel(pos[0], pos[1]);
		calib_nine_->addPoint_robot(pos[2], pos[3]);
	}

	double rms = calib_nine_->calibration();
	auto rmsEdit = findChild<QLineEdit*>("resultrms");
	rmsEdit->setText(QString::number(rms, 'f', 2));
}

void CalibrationDlg::on_save_calibData()
{
	
	QString calibFile = QFileDialog::getSaveFileName(nullptr, "保存标定文件", "calib_data", "*.bin");
	if (calibFile.isEmpty())
	{
		qInfo() << "保存标定文件失败,路径为空";
		return;
	}

	calib_nine_->writeTransFile(calibFile.toStdString().c_str());
}

void CalibrationDlg::clearTable()
{
	auto tab = findChild<QTableWidget*>("calibDataTab");
	tab->clearContents();
	tab->setRowCount(0);
}
